using BepuUtilities.Collections;
using BepuPhysics;
using BepuPhysics.Constraints;
using BepuUtilities;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif
namespace DemoRenderer.Constraints
{
    struct BallSocketMotorLineExtractor : IConstraintLineExtractor<BallSocketMotorPrestepData>
    {
        public int LinesPerConstraint => 2;

        public unsafe void ExtractLines(ref BallSocketMotorPrestepData prestepBundle, int setIndex, int* bodyIndices,
            Bodies bodies, ref Vector3 tint, ref QuickList<LineInstance> lines)
        {
            // 可以一次做一大堆约束,但是嗯。
            var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]];
            var poseB = bodies.Sets[setIndex].Poses[bodyIndices[1]];
            Vector3Wide.ReadFirst(prestepBundle.LocalOffsetB, out var localOffsetB);
            QuaternionEx.Transform(localOffsetB, poseB.Orientation, out var worldOffsetB);
            var anchor = poseB.Position + worldOffsetB;
            var color = new Vector3(0.2f, 0.2f, 1f) * tint;
            var packedColor = Helpers.PackColor(color);
            var backgroundColor = new Vector3(0f, 0f, 1f) * tint;
            lines.AllocateUnsafely() = new LineInstance(poseA.Position, anchor, packedColor, 0);
            lines.AllocateUnsafely() = new LineInstance(poseB.Position, anchor, packedColor, 0);
        }
    }
}
